#include "icm_imu_driver.h"



Imu_Init_list imu2_init_list = {
.now_num = 0
};

Imu_Init_list imu1_init_list = {
	.now_num = 0
};

void REGISTER_IMU_INIT_PACK(Imu_Init_list *imu_list, uint8_t init_reg, uint8_t from_reg, REG_OPERATE_WAY way, uint8_t init_value, uint16_t delay_time){
	if(imu_list->now_num < MAX_INIT_PACK_NUM){
		imu_list->init_packs[imu_list->now_num].init_reg = init_reg;
		imu_list->init_packs[imu_list->now_num].from_reg = from_reg;
		imu_list->init_packs[imu_list->now_num].value = init_value;
		imu_list->init_packs[imu_list->now_num].way = way;
		imu_list->init_packs[imu_list->now_num].init_delay_time = delay_time;
		imu_list->now_num++;
	}
}


uint8_t set_icm_reg(Imu_driver_t *imu_driver, uint8_t reg_addr, uint8_t reg_value) {
    return IMU_SPI_ReadWriteBytes(imu_driver, reg_addr, reg_value, NULL, 1);
}

uint8_t get_icm_reg(Imu_driver_t *imu_driver, uint8_t reg_addr, uint8_t* buf, uint8_t read_size){
    return IMU_SPI_ReadWriteBytes(imu_driver, reg_addr, 0, &buf[0], read_size);
}

uint8_t IMU_SPI_ReadWriteBytes(Imu_driver_t *imu_driver, uint8_t addr, uint8_t txData, uint8_t *rxBuf, uint16_t len){
		if (HAL_SPI_GetState(imu_driver->hspi_use) != HAL_SPI_STATE_READY){
			return 1;
		}
		HAL_GPIO_WritePin(imu_driver ->CS_PORT, imu_driver ->CS_PIN, GPIO_PIN_RESET);  
		uint16_t total = len + 1;
    uint8_t tx[total], rx[total];
    if (rxBuf) {
        tx[0] = addr | 0x80;
        memset(&tx[1], 0xFF, len);
    } else {
        tx[0] = addr & 0x7F;
        tx[1] = txData;
    }
    
    if (HAL_SPI_TransmitReceive(imu_driver->hspi_use, tx, rx, total, HAL_MAX_DELAY) != HAL_OK) {
			HAL_GPIO_WritePin(imu_driver ->CS_PORT, imu_driver ->CS_PIN, GPIO_PIN_SET);
			return 1;
		}

    if (rxBuf) {
			memcpy(rxBuf, &rx[1], len);
		}
		HAL_GPIO_WritePin(imu_driver ->CS_PORT, imu_driver ->CS_PIN, GPIO_PIN_SET);
    return 0;
}

void ICM_ReadID(Imu_driver_t *imu_driver) {
    uint8_t id = 0;
    if (IMU_SPI_ReadWriteBytes(imu_driver, DEVICE_ID_REG, 0, &id, 1)){
			return;
		};
		if(id == DEVICE_ID){
			imu_driver->imu_online_if = true;
			imu_driver->imu_state = IMU_INIT;
		}
}

uint8_t read_reg[10] = {0x00};

void ICM_Init(Imu_driver_t *imu_driver) {
	Imu_Init_list* operate_list;
	if(imu_driver->imu_id == 1){
		operate_list = &imu1_init_list;
	}else{
		operate_list = &imu2_init_list;
	}
	if(operate_list->now_init_step < operate_list->now_num){
		if(operate_list->init_packs[operate_list->now_init_step].way == SET_REG){
			read_reg[0] = set_icm_reg(imu_driver, operate_list->init_packs[operate_list->now_init_step].init_reg, operate_list->init_packs[operate_list->now_init_step].value);
		}
		else if(operate_list->init_packs[operate_list->now_init_step].way == GET_REG){
			read_reg[0] = get_icm_reg(imu_driver, operate_list->init_packs[operate_list->now_init_step].init_reg, &operate_list->init_packs[operate_list->now_init_step].value, 1);
		}else if(operate_list->init_packs[operate_list->now_init_step].way == SET_FROM_REG){
			uint8_t reg_val = 0xFF;
			for(int i=0;i < operate_list->now_init_step; i++){
				if(operate_list->init_packs[i].init_reg == operate_list->init_packs[operate_list->now_init_step].from_reg){
					reg_val = operate_list->init_packs[i].value;
				}
			}
			read_reg[0] = set_icm_reg(imu_driver, operate_list->init_packs[operate_list->now_init_step].init_reg, imu2_init_list.init_packs[operate_list->now_init_step].value | reg_val);
		}
		if(read_reg[0] == 0){
				operate_list->now_init_step ++;
			}
			IMU_DELAY(operate_list->init_packs[operate_list->now_init_step].init_delay_time);
	}
    if (operate_list->now_init_step == operate_list->now_num) {
        imu_driver->imu_state = IMU_IDLE;
    }
}

void calc_imu_offset(Imu_driver_t *imu_driver){
	if(imu_driver->now_offset_data_get < imu_driver->max_offset_data_get){
		if(imu_driver->now_offset_data_get == 0){
			imu_driver->get_gyro_x_data = imu_driver->imu_original_data.gx - imu_driver->gyro_x_offset;
			imu_driver->get_gyro_y_data = imu_driver->imu_original_data.gy - imu_driver->gyro_y_offset;
			imu_driver->get_gyro_z_data = imu_driver->imu_original_data.gz - imu_driver->gyro_z_offset;
			imu_driver->get_accel_x_data = imu_driver->imu_original_data.ax - imu_driver->accel_x_offset;
			imu_driver->get_accel_y_data = imu_driver->imu_original_data.ay - imu_driver->accel_y_offset;
			imu_driver->get_accel_z_data = imu_driver->imu_original_data.az - imu_driver->accel_z_offset;
			imu_driver->accel_x_offset = 0;
			imu_driver->accel_y_offset = 0;
			imu_driver->accel_z_offset = 0;
			imu_driver->gyro_x_offset = 0;
			imu_driver->gyro_y_offset = 0;
			imu_driver->gyro_z_offset = 0;
		}else{
			imu_driver->get_gyro_x_data = (imu_driver->imu_original_data.gx + imu_driver->get_gyro_x_data) / 2;
			imu_driver->get_gyro_y_data = (imu_driver->imu_original_data.gy + imu_driver->get_gyro_y_data) / 2;
			imu_driver->get_gyro_z_data = (imu_driver->imu_original_data.gz + imu_driver->get_gyro_z_data) / 2;
			imu_driver->get_accel_x_data = (imu_driver->imu_original_data.ax + imu_driver->get_accel_x_data) / 2;
			imu_driver->get_accel_y_data = (imu_driver->imu_original_data.ay + imu_driver->get_accel_y_data) / 2;
			imu_driver->get_accel_z_data = (imu_driver->imu_original_data.az + imu_driver->get_accel_z_data) / 2;
		}
		imu_driver->now_offset_data_get ++;
	}else{
		imu_driver->update_offset = false;
		imu_driver->now_offset_data_get = 0;
		imu_driver->gyro_x_offset = -imu_driver->get_gyro_x_data;
		imu_driver->gyro_y_offset = -imu_driver->get_gyro_y_data;
		imu_driver->gyro_z_offset = -imu_driver->get_gyro_z_data;
		imu_driver->accel_x_offset = -imu_driver->get_accel_x_data;
		imu_driver->accel_y_offset = -imu_driver->get_accel_y_data;
		imu_driver->accel_z_offset = -imu_driver->get_accel_z_data;
//		if(imu_driver->imu_original_data.az > 0){
//			imu_driver->accel_z_offset = G - imu_driver->get_accel_z_data;
//		}else{
//			imu_driver->accel_z_offset = -(G + imu_driver->get_accel_z_data);
//		}
		
		uint8_t raw_data[30];
		if(imu_driver->imu_id == 2){
			raw_data[0] = IMU2_GYRO_X_OFFSET;
			raw_data[5] = IMU2_GYRO_Y_OFFSET;
			raw_data[10] = IMU2_GYRO_Z_OFFSET;
			raw_data[15] = IMU2_ACCEL_X_OFFSET;
			raw_data[20] = IMU2_ACCEL_Y_OFFSET;
			raw_data[25] = IMU2_ACCEL_Z_OFFSET;
		}else if(imu_driver->imu_id == 1){
			raw_data[0] = IMU1_GYRO_X_OFFSET;
			raw_data[5] = IMU1_GYRO_Y_OFFSET;
			raw_data[10] = IMU1_GYRO_Z_OFFSET;
			raw_data[15] = IMU1_ACCEL_X_OFFSET;
			raw_data[20] = IMU1_ACCEL_Y_OFFSET;
			raw_data[25] = IMU1_ACCEL_Z_OFFSET;
		}
		memcpy(&raw_data[1], (void*)(GET_RAM_ADDR(imu_driver->gyro_x_offset)), 4);
		memcpy(&raw_data[6], (void*)(GET_RAM_ADDR(imu_driver->gyro_y_offset)), 4);
		memcpy(&raw_data[11], (void*)(GET_RAM_ADDR(imu_driver->gyro_z_offset)), 4);
		memcpy(&raw_data[16], (void*)(GET_RAM_ADDR(imu_driver->accel_x_offset)), 4);
		memcpy(&raw_data[21], (void*)(GET_RAM_ADDR(imu_driver->accel_y_offset)), 4);
		memcpy(&raw_data[26], (void*)(GET_RAM_ADDR(imu_driver->accel_z_offset)), 4);
		update_param_data(&raw_data[0], 30);
	}
}

void imu_idle(Imu_driver_t *imu_driver){
	get_icm_reg(imu_driver, 0x1F, imu_driver->imu_reg_data, 12);
	imu_driver->now_imu_frame.ax = (int16_t)((imu_driver->imu_reg_data[0] << 8 | imu_driver->imu_reg_data[1]));
	imu_driver->now_imu_frame.ay = (int16_t)((imu_driver->imu_reg_data[2] << 8 | imu_driver->imu_reg_data[3]));
	imu_driver->now_imu_frame.az = (int16_t)((imu_driver->imu_reg_data[4] << 8 | imu_driver->imu_reg_data[5]));
					
	imu_driver->now_imu_frame.gx = (int16_t)((imu_driver->imu_reg_data[6] << 8 | imu_driver->imu_reg_data[7]));
	imu_driver->now_imu_frame.gy = (int16_t)((imu_driver->imu_reg_data[8] << 8 | imu_driver->imu_reg_data[9]));
	imu_driver->now_imu_frame.gz = (int16_t)((imu_driver->imu_reg_data[10] << 8 | imu_driver->imu_reg_data[11]));
				
	imu_driver->imu_original_data.ax = imu_driver->now_imu_frame.ax * imu_driver->accel_dps + imu_driver->accel_x_offset;
	imu_driver->imu_original_data.ay = imu_driver->now_imu_frame.ay * imu_driver->accel_dps + imu_driver->accel_y_offset;
	imu_driver->imu_original_data.az = imu_driver->now_imu_frame.az * imu_driver->accel_dps + imu_driver->accel_z_offset;
					
	imu_driver->imu_original_data.gx = imu_driver->now_imu_frame.gx * imu_driver->gyro_dps + imu_driver->gyro_x_offset ;
	imu_driver->imu_original_data.gy = imu_driver->now_imu_frame.gy * imu_driver->gyro_dps + imu_driver->gyro_y_offset;
	imu_driver->imu_original_data.gz = imu_driver->now_imu_frame.gz * imu_driver->gyro_dps + imu_driver->gyro_z_offset;
	
	if(imu_driver->update_offset){
		calc_imu_offset(imu_driver);
	}
}


void imu_auto(Imu_driver_t *imu_driver){
	switch(imu_driver->imu_state){
		case FIND_IMU:
			ICM_ReadID(imu_driver);
			if(imu_driver->imu_id == 1){
				imu1_init_list.now_init_step = 0;
			}else{
				imu2_init_list.now_init_step = 0;
			}
			break;
		case IMU_INIT:
			ICM_Init(imu_driver);
			break;
		case IMU_IDLE:
			imu_idle(imu_driver);
			break;
		default:
			break;
	}
}


